#include "includes.h"
#include "drivers.h"
#include "data.h"
#include "filter.h"
#include "control.h"

extern uint16 amp[4],cnt[2];

void pit0_isr(void){
    /* Control routine */
    static int t;

    servo_control();

    if(t++==5){
        motor_control();
        t=0;
    }

    implementation();

    PIT_CLEAR_FLAG(0);
}

/* amp[0]: left(C2),
 * amp[1]: right(B1)
 * amp[2]: left_middle(E20)
 * amp[3]: right_middle(E22)
 *
 * cnt[0]: left(B16)
 * cnt[1]: right(C5)
 */
void pit1_isr(void){
    /* System routine */

    printf("%d|%d|%d|%d|%d#%d#\n", amp[0], amp[1], amp[2], amp[3], cnt[0], cnt[1]);

    PIT_CLEAR_FLAG(1);
}

void pit_isr(void){
    if(PIT_TFLG(0)&PIT_TFLG_TIF_MASK)
        pit0_isr();
    if(PIT_TFLG(1)&PIT_TFLG_TIF_MASK)
        pit1_isr();
}

void uart0_isr(void){
    /* Bluetooth command communication routine */
}

void dma1_isr(void){
    DMA_CLEAR_FLAG(1);
    amp_process();
    dma_adc_activate();
}
